#define HEAD_NECK1_CENTER 90
#define HEAD_NECK2_CENTER 92

#define ARM_SHOULDER_LEFT_CENTER 85
#define ARM_BICEP_LEFT_CENTER 95
#define ARM_ELBOW_LEFT_CENTER 90
#define ARM_WRIST_LEFT_CENTER 94
#define LEG_HIP1_LEFT_CENTER 90
#define LEG_HIP2_LEFT_CENTER 95
#define LEG_THIGH_LEFT_CENTER 92
#define LEG_KNEE_LEFT_CENTER 90
#define LEG_ANKLE_LEFT_CENTER 92

#define ARM_SHOULDER_RIGHT_CENTER 90
#define ARM_BICEP_RIGHT_CENTER 90
#define ARM_ELBOW_RIGHT_CENTER 90
#define ARM_WRIST_RIGHT_CENTER 94
#define LEG_HIP1_RIGHT_CENTER 90
#define LEG_HIP2_RIGHT_CENTER 90
#define LEG_THIGH_RIGHT_CENTER 90
#define LEG_KNEE_RIGHT_CENTER 97
#define LEG_ANKLE_RIGHT_CENTER 88

void set_position_right(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;
  head[I_HEAD_NECK2] = HEAD_NECK2_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER-60;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+90;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+90;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER+60;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-90;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-90;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER;
}

void set_position_stand(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;
  head[I_HEAD_NECK2] = HEAD_NECK2_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER-60;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+90;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+90;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER-10;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER-10;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER+60;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-90;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-90;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER-10;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER-10;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER;
}

void set_position_waitfor(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  set_position_stand(head, left_arm, left_leg, right_arm, right_leg);

  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+70;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+70;

  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER+5;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER-20;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER-20;

  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-70;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-70;

  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER-5;  
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER-20;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER-20;
}

void set_position_hello1(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  set_position_right(head, left_arm, left_leg, right_arm, right_leg);

  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+35;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+15;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER-90;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-70;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-70;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER-90;
}

void set_position_hello2(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  set_position_hello1(head, left_arm, left_leg, right_arm, right_leg);

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER-80;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-65;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-65;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER-45;  
}

void set_position_pump_high(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER+20;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+90;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+90;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER+8;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER+8;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER-20;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-90;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-90;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER-8;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER-8;
}

void set_position_pump_low(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  set_position_pump_high(head, left_arm, left_leg, right_arm, right_leg);

  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+50;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER;

  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-50;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER;
}

void set_position_semipump_high(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;
  head[I_HEAD_NECK2] = HEAD_NECK2_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+90;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+90;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER+10;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER-57;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-90;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-90;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER-10;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER+57;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER;
}

void set_position_semipump_low(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  set_position_semipump_high(head, left_arm, left_leg, right_arm, right_leg);

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER+3;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+45;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER;

  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER-5;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER-50;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER-3;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-45;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER;

  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER+5;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER+50;
}

void set_position_hulk(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;
  head[I_HEAD_NECK2] = HEAD_NECK2_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER-50;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER+40;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+50;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER-2;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER+10;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER+45;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER+45;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER+50;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER-40;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-50;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER+2;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER-10;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER+45;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER+45;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER;
}

void set_position_plane(int *head, int *left_arm, int *left_leg, int *right_arm, int *right_leg)
{
  head[I_HEAD_NECK1] = HEAD_NECK1_CENTER;
  head[I_HEAD_NECK2] = HEAD_NECK2_CENTER;

  left_arm[I_ARM_SHOULDER] = ARM_SHOULDER_LEFT_CENTER-60;
  left_arm[I_ARM_BICEP] = ARM_BICEP_LEFT_CENTER;
  left_arm[I_ARM_ELBOW] = ARM_ELBOW_LEFT_CENTER+90;
  left_arm[I_ARM_WRIST] = ARM_WRIST_LEFT_CENTER;

  left_leg[I_LEG_HIP1] = LEG_HIP1_LEFT_CENTER-90;
  left_leg[I_LEG_HIP2] = LEG_HIP2_LEFT_CENTER+90;
  left_leg[I_LEG_THIGH] = LEG_THIGH_LEFT_CENTER;
  left_leg[I_LEG_KNEE] = LEG_KNEE_LEFT_CENTER;
  left_leg[I_LEG_ANKLE] = LEG_ANKLE_LEFT_CENTER-90;

  right_arm[I_ARM_SHOULDER] = ARM_SHOULDER_RIGHT_CENTER+60;
  right_arm[I_ARM_BICEP] = ARM_BICEP_RIGHT_CENTER;
  right_arm[I_ARM_ELBOW] = ARM_ELBOW_RIGHT_CENTER-90;
  right_arm[I_ARM_WRIST] = ARM_WRIST_RIGHT_CENTER;

  right_leg[I_LEG_HIP1] = LEG_HIP1_RIGHT_CENTER+90;
  right_leg[I_LEG_HIP2] = LEG_HIP2_RIGHT_CENTER-90;
  right_leg[I_LEG_THIGH] = LEG_THIGH_RIGHT_CENTER;
  right_leg[I_LEG_KNEE] = LEG_KNEE_RIGHT_CENTER;
  right_leg[I_LEG_ANKLE] = LEG_ANKLE_RIGHT_CENTER+90;
}

